/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received _a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#pragma once

#include "Chain.h"

namespace Robotic { namespace Mechanism
{
	using namespace Robotic::Math;

	/*
	*/
	class ROBOTIC_MECHANISM_API Con_ResolvedAcc
	{
	public:
		Con_ResolvedAcc(const Chain* pRobot, const double Kvp, const double Kpp, const double Kvo, const double Kpo);
		void Set_Kvp(const double Kvp);
		void Set_Kpp(const double Kpp);
		void Set_Kvo(const double Kvo);
		void Set_Kpo(const double Kpo);

		/*
		@brief: calculates joint acceleration.
		@param: pd: desired joint value.
		@param: ppd: desired joint velocity.
		@param: pppd: desired joint acceleration.
		@param: wd: desired joint value.
		@param: wpd: desired joint value.
		@param: quatD: desired joint value.
		@returns: joint acceleration.
		*/
		ColumnVector Cmd_JointAcc(const ColumnVector & pd,
			const ColumnVector & ppd, const ColumnVector & pppd,
			const ColumnVector & wd, const ColumnVector & wpd,
			const Quaternion & quatD);

		ColumnVector Cmd_JointTorque(const ColumnVector & pd,
			const ColumnVector & ppd, const ColumnVector & pppd,
			const ColumnVector & wd, const ColumnVector & wpd,
			const Quaternion & quatD, const Vector3 & grav);
	private:
		const Chain* _pRobot;
		double _Kvp;                //!< Controller gains.
		double _Kpp; 
		double _Kvo; 
		double _Kpo; 
	};
}}